from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_nodes(context, *args, **kwargs):
    # 获取动态参数值
    vehicle_number = int(LaunchConfiguration('vehicle_number').perform(context))
    namespace = LaunchConfiguration('namespace').perform(context)

    # 创建节点列表
    nodes = []
    for i in range(vehicle_number):
        node = Node(
            package='swarm_avoid_client',
            executable='node',
            name=f'node_{i}',
            namespace=namespace,
            output="screen",
            emulate_tty=True,
            parameters=[
                {"vehicle_id": i},
                {"namespace": namespace},
                {"vehicle_number": vehicle_number},
            ]
        )
        nodes.append(node)
    return nodes

def generate_launch_description():
    # 声明参数
    vehicle_number_arg = DeclareLaunchArgument(
        'vehicle_number',
        default_value="3",
        description='需要启动的车辆节点总数'
    )
    namespace_arg = DeclareLaunchArgument(
        'namespace',
        default_value="/drone",
        description='节点的命名空间'
    )

    # 使用 OpaqueFunction 动态生成节点
    return LaunchDescription([
        vehicle_number_arg,
        namespace_arg,
        OpaqueFunction(function=generate_nodes),
    ])
